from direct.showbase.ShowBase import ShowBase
from direct.task import Task
from panda3d.core import Camera, PointLight, WindowProperties
from test_euler import *
import load_data

whl_list = ["wheel/wheel11.obj", "wheel/wheel22.obj", "wheel/wheel33.obj", "wheel/wheel44.obj"]
long_w = [7.4, -7.4, 7.4, -7.4]
lat_w = [4.2, 4.2, -4.2, -4.2]
ang_w = [0, 0, 180, 180]
sgn_w = [1, 1, -1, -1]
len_cam = [40, 150, 40, 150]
light_list = [(0, 40),  (0, -40), (40, 0), (-40, 0)]
n_wheel = 4
n_cam = 4
/ m,,.

class MyApp(ShowBase):
    def __init__(self, ang0=np.array([50, 105, 195, 285])):

        super().__init__()
        self.ang00 = ang0
        self.ang01 = ang0 * np.pi / 180
        self.cam_list = [self.cam]
        # 获取窗口尺寸
        win_width = self.win.getXSize()
        win_height = self.win.getYSize()
        prop = WindowProperties()
        prop.setSize(1600, 1200)
        self.win.requestProperties(prop)
        self.disable_mouse()

        # Load the environment model.
        self.scene = self.loader.loadModel("alice-skies--happysky/happysky.egg")
        self.scene.reparentTo(self.render)
        self.scene.setScale(.25, .25, 1)
        self.land = self.loader.loadModel("alice-farm--cornfield/cornfield.egg")
        self.land.reparentTo(self.render)
        self.land.setScale(20, 20, 1)
        self.land.setPos(0, 0, -5)

        # 添加点光源
        for _ in range(n_cam):
            light = PointLight('light')
            light_node = self.render.attachNewNode(light)
            light_node.setPos(light_list[_][0], light_list[_][1], 10)
            light.setColor((1, 1, 1, 0.5))  # 白色光
            self.render.setLight(light_node)

        # 创建主相机（默认相机）
        for _ in range(1, n_cam):
            cam = Camera(str(_) + "-camera")
            self.cam_list.append(self.render.attachNewNode(cam))
        # 清除所有现有显示区域
        self.win.removeAllDisplayRegions()
        for _ in range(n_cam):
            self.cam_list[_].setPos(40 * np.sin(self.ang01[_]),
                                    -len_cam[_] * np.cos(self.ang01[_]), 4)
            # self.cam_list[_].setHpr(self.ang00[_], 0, 0)
            self.cam_list[_].lookAt(0, 0, 0)
            # 创建显示区域（相机）
            left_dr = self.win.makeDisplayRegion(int(_ / 2) / 2.0, int(_ / 2) / 2.0 + 0.5,
                                                 int(_ % 2) / 2.0, int(_ % 2) / 2.0 + 0.5)
            left_dr.setCamera(self.cam_list[_])

        self.veh = self.loader.loadModel("veh_body/body.obj")
        self.wheel = []
        for _ in range(n_wheel):
            self.wheel.append(self.loader.loadModel(whl_list[_]))

        mult = 500
        self.veh.setScale(mult, mult, mult)
        self.veh.setHpr(0, 90, 0)
        self.veh.setPos(0, 0, 0)
        self.veh.reparentTo(self.render)
        for _ in range(n_wheel):
            self.wheel[_].setScale(5, 5, 5)
            self.wheel[_].reparentTo(self.render)
            self.wheel[_].setPos(lat_w[_], long_w[_], 1.2)
            self.wheel[_].setHpr(ang_w[_], 0, 0)
        print("当前显示区域数量:", self.win.getNumDisplayRegions())

        self.taskMgr.add(self.car_move, "car_move")

    def car_move(self, task):
        time0 = task.time % load_data.total_time
        t_step = int(time0 * load_data.freq) % load_data.len_anime

        pitch_ang = load_data.pitch_body[t_step]
        roll_ang = load_data.roll_body[t_step]
        yaw_ang = load_data.yaw_body[t_step]
        z_body = load_data.z_body[t_step]

        w_rl = load_data.ang_rl[t_step]
        w_fl = load_data.ang_fl[t_step]
        w_rr = load_data.ang_rr[t_step]
        w_fr = load_data.ang_fr[t_step]

        spin_rl = load_data.spin_rl[t_step]
        spin_fl = load_data.spin_fr[t_step]
        spin_rr = load_data.spin_rr[t_step]
        spin_fr = load_data.spin_fr[t_step]

        z_rl = load_data.z_rl[t_step]
        z_fl = load_data.z_fl[t_step]
        z_rr = load_data.z_rr[t_step]
        z_fr = load_data.z_fr[t_step]

        y0 = -yaw_ang * np.pi / 180
        e1 = [yaw_ang, 90 + roll_ang, 0]
        e2 = [pitch_ang, 0, 0]
        a1, a2, a3 = equivalent_rotation(e1, e2, '3d_euler')
        w_ang = [w_rl, w_fl, w_rr, w_fr]
        whl_ang = [spin_rl, spin_fl, spin_rr, spin_fr]
        z_whl = [z_rl, z_fl, z_rr, z_fr]

        self.veh.setPos(0, 0, z_body)
        self.veh.setHpr(a1, a2, a3)
        for _ in range(n_wheel):
            self.wheel[_].setPos(lat_w[_] * np.cos(y0) + long_w[_] * np.sin(y0),
                                 long_w[_] * np.cos(y0) - lat_w[_] * np.sin(y0), 1.2 + z_whl[_])
            self.wheel[_].setHpr(yaw_ang + w_ang[_], whl_ang[_] * sgn_w[_], ang_w[_])

        return Task.cont


app = MyApp()
app.run()
